#include <motion_controller/controller/processor_p2p.h>


// 构造函数
ProcessorP2P::ProcessorP2P(boost::shared_ptr<PlannerBase> path_planner_ptr,
            boost::shared_ptr<ControllerBase> localControllerPtr):
    globalPathPlannerPtr_(path_planner_ptr), localControllerPtr_(localControllerPtr){
    __rotate_start_direction = false;
    ResetIdle();
}

// 析构函数
ProcessorP2P::~ProcessorP2P(){

}

// 设置目标点数据
void ProcessorP2P::setGoal(geometry_msgs::PoseStamped pose_stamp){
    GLOG_INFO << "[ProcessorP2P] set goal pose!";
    goalPoseStamp_ = pose_stamp;
    setGoalPose_ = true;
}

// 空闲状态
void ProcessorP2P::ResetIdle(){
    pathGlobal_.clear();
}

// 执行中预处理函数
void ProcessorP2P::ExecutingEntry(){
    if(!setGoalPose_){
        GLOG_WARN << "please set goal pose!";
        setEvent(StateMachine::Event::EV_CANCEL);
        return;
    }
    if(!globalPathPlannerPtr_){
        GLOG_WARN << "planner_ptr is nullptr!";
        setEvent(StateMachine::Event::EV_CANCEL);
        return;
    }

    // 生成路径
    vector<geometry_msgs::Pose> poses;
    mc_status gen_state = globalPathPlannerPtr_->pathPlanning(robotPoseStamp_.pose, goalPoseStamp_.pose, poses);
    __rotate_start_direction = false;
    if(!gen_state.success || poses.empty()){
        LOG(WARNING) << "generate path failed!";
        setEvent(StateMachine::Event::EV_CANCEL);
        return;    
    }
    pathGlobal_.resize(poses.size());
    for(size_t i = 0; i < poses.size(); ++i){
        pathGlobal_[i] = msg_convert::toPoseStamped(poses[i], "map");
    }
    cout << " global_path front: " << msg_convert::toString(pathGlobal_.front().pose) << 
        ", back: " << msg_convert::toString(pathGlobal_.back().pose) << endl;
}

// 执行中处理函数
void ProcessorP2P::ExecutingRunning(){
    try{
        geometry_msgs::Twist cmd_vel;

        if(pathGlobal_.empty()){
            throw std::runtime_error("pathGlobal_ is empty!");
        }      

        // 全局路径转局部路径，并裁剪在有效范围内
        pathLocal_ =  localControllerPtr_->convertGlobalPathToLocal(pathGlobal_, robotPoseStamp_);
        pathLocal_ =  localControllerPtr_->pruningLocalPath(pathLocal_, 2.0, 2.0);
        localControllerPtr_->publishLocalPath(pathLocal_);

        // 获取局部前瞻目标点
        double lookahead_distance = localControllerPtr_->getLookAheadDistance(cmd_vel.linear.x);
        geometry_msgs::PoseStamped lookahead_goal;
        if(!localControllerPtr_->getLookaheadGoal(pathLocal_, lookahead_distance, lookahead_goal)){
            throw std::runtime_error("can not get lookahead goal!");
        }
        localControllerPtr_->publishLookaheadGoal(lookahead_goal);
        cout << "[ExecutingRunning] lookahead_distance: " << lookahead_distance << ", lookahead_goal: " << msg_convert::toString(lookahead_goal.pose) << endl;

        // 判断是否到达终点
        geometry_msgs::PoseStamped robot_local_pose = localControllerPtr_->getOriginPose();
        if(localControllerPtr_->isGoalReached(robot_local_pose, lookahead_goal)){
            ROS_INFO("The robot reaches the goal point.");
            localControllerPtr_->sendEmptyTwist();
            setEvent(StateMachine::Event::EV_OVER);
            return;
        }   

        // 起点角度旋转, 减少不必要转弯次数
        if(!__rotate_start_direction){
            cout << "__rotate_start_direction: " << __rotate_start_direction << endl;
            if(!localControllerPtr_->isAngleReached(robot_local_pose, lookahead_goal)){
                localControllerPtr_->rotateToTGoal(robot_local_pose, lookahead_goal, cmd_vel);
                cout << "[ExecutingRunning | rotateToTGoal] cmd_vel: " << msg_convert::toString(cmd_vel) << endl;
                localControllerPtr_->sendVelocity(cmd_vel);
            }
            else{
                __rotate_start_direction = true;
                localControllerPtr_->sendEmptyTwist();
            }
            return;
        }        

        // 执行局部路径
        mc_status status = 
            localControllerPtr_->computeTwist(lookahead_goal, cmd_vel);
        cout << "[ExecutingRunning | computeTwist] cmd_vel: " << msg_convert::toString(cmd_vel) << endl;
        if(status.success){
            localControllerPtr_->sendVelocity(cmd_vel);
        }else{
            setEvent(StateMachine::Event::EV_OVER);
        }

    }
    catch (const std::runtime_error& e) {
        std::cerr << "[ProcessorP2P | ExecutingRunning] error: " << e.what() << std::endl;
        localControllerPtr_->sendEmptyTwist();
    }

}

void ProcessorP2P::ExecutingExit(){
    // setGoalPose_ = false;
    // localControllerPtr_->PublishEmptyTwist();
}